/****************************************************************************/
/* V4L2 Character Device Driver Module (HW3)	                       		*/
/* Programmed by Ishai Jaffe (039713839) and Iddo Lazar (036829539)			*/
/****************************************************************************/

/*****************************************************************************/
/********************************* INCLUDES **********************************/
/*****************************************************************************/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/fs.h>
#include <asm/uaccess.h>
#include <linux/mutex.h>
#include <linux/ioctl.h>
#include <net/netlink.h>
#include <net/genetlink.h>
#include <linux/videodev.h>
#include "common.h"

/*****************************************************************************/
/******************************** MACROS *************************************/
/*****************************************************************************/
#define MCG_NAME     "STD_MCG"                      /* Standard multicast group */

#define _DEBUG_NOT

#ifdef _DEBUG
#	define printd(x...) printk(KERN_DEBUG "DEBUG: " x) /* Debugging function */
#else
#	define printd(x...) ((void)(0))
#endif

// size of packets queue
#define DRIVER_QUEUE	200
// minimum size of queue to start recieving frame from camera
#define EMPTY_PACKET_TRESHOLD	70

/*****************************************************************************/
/*************************** TYPE DEFINITIONS ********************************/
/*****************************************************************************/
typedef struct
{
	unsigned int length;
	char msg[NL_PACKET_SIZE];
} net_link_data;

typedef struct
{
	unsigned int first, last, count;
	unsigned int last_unfilled;
	unsigned int count_unfilled;
	int is_frame_requested;
	void 		 *buffers[DRIVER_QUEUE];
	net_link_data *nl_buffers[DRIVER_QUEUE];

} driver_queue_t;


/*****************************************************************************/
/******************************** PROTOTYPES *********************************/
/*****************************************************************************/

int 			init_queue_buffers();
void 			fill_buffer();
net_link_data 	*read_last_buffer();
int 			enqueue_buffer(void *buff);
void 			*dequeue_buffer();

int		device_ioctl(struct inode *inode, struct file *file, unsigned int ioctl_num, unsigned long ioctl_param);
static int     device_open(struct inode *, struct file *);
static int     device_release(struct inode *, struct file *);
static ssize_t device_read(struct file *, char *, size_t, loff_t *);
static ssize_t device_write(struct file *, const char *, size_t, loff_t *);
static int     msg_cb(struct sk_buff *skb, struct genl_info *info);
static int     send_msg(void *buff, unsigned int len, int attr);




/*****************************************************************************/
/************************ FILE OPERATION DEFINITIONS *************************/
/*****************************************************************************/

MODULE_DESCRIPTION("Gaylord fuckers driver");
MODULE_AUTHOR("Stiffler's mom");
MODULE_LICENSE("GPL");

struct file_operations Fops = {
	.read    = device_read,
	.write   = device_write,
	.ioctl   = device_ioctl,
	.open    = device_open,
	.release = device_release
};

/*****************************************************************************/
/*************************** NETLINK DEFINITIONS *****************************/
/*****************************************************************************/
/* Family definition */
static struct genl_family ex_genl_family = {
      .id      = GENL_ID_GENERATE,
      .hdrsize = FAMILY_HEADER_LEN,
      .name    = FAMILY_NAME,
      .version = 1,
      .maxattr = MAX_ATTR_INDEX ,
};

/* Multicast group */
static struct genl_multicast_group ex_mc_grp = { .name = MCG_NAME };

/* Attribures */
static struct nla_policy ex_policy[MAX_ATTR_INDEX+1] =
	{ [INIT_ATTR] 		= { .type = NLA_BINARY },
	  [KILL_ATTR]		= { .type = NLA_BINARY },
	  [STREAMON_ATTR]	= { .type = NLA_BINARY },
	  [STREAMOFF_ATTR]	= { .type = NLA_BINARY },
	  [FRAME_ATTR]     	= { .type = NLA_BINARY },
	  [RETURN_ATTR]		= { .type = NLA_BINARY }
	};

/* Operation definition */
static struct genl_ops ex_gops = {
      .cmd = STD_CMD,
      .flags = 0,
      .policy = ex_policy,
      .doit = msg_cb,
      .dumpit = NULL,
};

/*****************************************************************************/
/***************************** GLOBAL VARIABLES ******************************/
/*****************************************************************************/
int    deviceOpen = 0;      /* Device opened. Prevents multiple accesses. */
struct net 	*net;
char *fmt_ptr;
int retValue;
net_link_data data;
driver_queue_t driver_queue;
void *mem;

DECLARE_COMPLETION(cond);
DEFINE_MUTEX(queue_mutex);

/*****************************************************************************/
/***************************** QUEUE Related    ******************************/
/*****************************************************************************/

int init_queue_buffers()
{
	int i;
	// how many full buffers in queue
	driver_queue.count = 0;
	// how many empty buffers in queue
	driver_queue.count_unfilled = 0;
	// index of first full buffer
	driver_queue.first = -1;
	// index of the buffer to fill next
	driver_queue.last = 0;
	// index to insert next empty buffer
	driver_queue.last_unfilled = 0;

	driver_queue.is_frame_requested = 0;

	for(i=0; i<DRIVER_QUEUE; i++)
	{
		driver_queue.nl_buffers[i] = (net_link_data *)(mem + i*sizeof(net_link_data));
		driver_queue.nl_buffers[i]->length = 1;
		driver_queue.nl_buffers[i]->msg[0] = 0;
	}
	mutex_init(&queue_mutex);
	return 0;
}

// after filling the buffer, increment first empty buffer index (from Camera)
void fill_buffer()
{
	int nextIndex = (driver_queue.last + 1) % DRIVER_QUEUE;
	mutex_lock(&queue_mutex);
	driver_queue.last = nextIndex;
	driver_queue.count++;
	driver_queue.count_unfilled--;
	if(driver_queue.first == -1)
		driver_queue.first = 0;
	mutex_unlock(&queue_mutex);
}

// read the first empty buffer, in order to fill it ( from Camera)
net_link_data *read_last_buffer()
{
	if(driver_queue.count_unfilled == 0)
	{
		printk("Buffer thrown away\n");
		//wait_for_completion(&queue_cond);
		return NULL;
	}
	return driver_queue.nl_buffers[driver_queue.last];
}

// enqueue an empty buffer to the queue ( from FG)
int enqueue_buffer(void *buff)
{
	if(driver_queue.count + driver_queue.count_unfilled == DRIVER_QUEUE)
	{
		return -1;
	}
	mutex_lock(&queue_mutex);
	driver_queue.buffers[driver_queue.last_unfilled] = buff;
	driver_queue.last_unfilled = (driver_queue.last_unfilled + 1) % DRIVER_QUEUE;
	driver_queue.count_unfilled++;
	mutex_unlock(&queue_mutex);
	return 0;
	//complete(&queue_cond);
}

// get the first full buffer in the queue (from FG)
void *dequeue_buffer()
{
	int retIndex;
	if(driver_queue.count == 0)
	{
		return NULL;
	}
	retIndex = driver_queue.first;
	copy_to_user(driver_queue.buffers[retIndex],
			driver_queue.nl_buffers[retIndex]->msg,
			driver_queue.nl_buffers[retIndex]->length);

	mutex_lock(&queue_mutex);
	driver_queue.first += 1;
	driver_queue.first %= DRIVER_QUEUE;
	driver_queue.count--;
	mutex_unlock(&queue_mutex);
	return driver_queue.buffers[retIndex];
}

/*****************************************************************************/
/**************************** NETLINK SENDING ********************************/
/*****************************************************************************/
static int send_msg(void *buff, unsigned int len, int attr)
{
	struct sk_buff *skb;
	void *msg_head;
	int rc = -1;
	printd("Inside send_msg\n");
	if( net == NULL)
		return -1;
	/* Make a new message */
	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
	if (skb == NULL)
	{
		rc = -ENOMEM;
		goto msg_err;
	}

	/* Create the message headers */
	msg_head = genlmsg_put(skb, NETLINK_PID_SOCK, 0, &ex_genl_family, NLM_F_MULTI, 0);
	if (msg_head == NULL) {
		rc = -ENOMEM;
		goto msg_err;
	}

	/* Filling the message with the right attribute */
	rc = nla_put(skb, attr, len, buff);
	if (rc != 0)
		goto msg_err;

	/* Finalize the message */
	genlmsg_end(skb, msg_head);
	if(net == NULL)
	{
		rc = -ENOMEM;
		goto msg_err;
	}

	rc = genlmsg_unicast(net, skb, NETLINK_PID_SOCK);

	goto end;

	/* Error goto label */
msg_err:
	if (skb == NULL)
		goto end_error;
	if (msg_head != NULL)
		genlmsg_cancel(skb, msg_head);
	nlmsg_free(skb);
end_error:
	return -1;

	/* Successful sending goto label */
end:
	return 0;
}

/*****************************************************************************/
/**************************** NETLINK CALLBACK *******************************/
/*****************************************************************************/
static int msg_cb(struct sk_buff *skb, struct genl_info *info)
{
	int            len      = nlmsg_len(info->nlhdr); /* Length of header */
	struct nlattr *attrs1   = NULL; /* The temp place to hold the received attribute */
	char          *rec_data = nlmsg_data(info->nlhdr); /* Data received in the message */
	net_link_data *data_to_fill;
	int stam=0;
	//save net for send_msg
	net = info->_net;

	printd("---START NETLINK MSG CALLBACK---\n");


	if (rec_data == NULL)
	{
		printd("Error - The data is NULL!\n");
		return -1;
	}

	if (len < 0)
	{
		printd("Error - The length of the message is negative!\n");
		return -1;
	}
	printd("NETLINK -->  msg_cb::DATA LENGTH: %d\n", len);
	/* Check if received data (chunk) after a read message request OR a return value*/
	if(info->attrs[FRAME_ATTR])
	{
		printd("NETLINK -->  msg_cb::FRAME_ATTR\n");
		attrs1 = info->attrs[FRAME_ATTR];
		rec_data = nla_data(attrs1);

		data_to_fill = read_last_buffer();
		if( data_to_fill != NULL)
		{
			memset(data_to_fill->msg,0,data_to_fill->length);
			memcpy(data_to_fill->msg, rec_data,len-8);
			data_to_fill->length = len-8;
			fill_buffer();
		}
		printd("NETLINK -->  FRAME_ATTR filled count :%d \n", driver_queue.count);
		printd("NETLINK -->  FRAME_ATTR unfilled count :%d \n", driver_queue.count_unfilled);
	}
	else if (info->attrs[STREAMON_ATTR])
	{
		printd("NETLINK -->  msg_cb::STREAMON_ATTR\n");

		attrs1 = info->attrs[STREAMON_ATTR];
		rec_data = nla_data(attrs1);
		memset(data.msg, 0, data.length);
		memcpy(data.msg, rec_data,len-8);
		data.length = len-8;

		complete(&cond);
	}
	else if(info->attrs[G_FMT_ATTR])
	{
		printd("NETLINK -->  msg_cb::G_FMT_ATTR\n");

		attrs1 = info->attrs[G_FMT_ATTR];
		rec_data = nla_data(attrs1);

		memset(data.msg, 0, data.length);
		memcpy(data.msg, rec_data,len-8);
		data.length = len-8;

		complete(&cond);
	}
	// when the camera sent frame finish signal
	else if(info->attrs[RETURN_ATTR])
	{
		printd("NETLINK -->  msg_cb::RETURN_ATTR\n");

		attrs1 = info->attrs[RETURN_ATTR];
		rec_data = nla_data(attrs1);


		if( driver_queue.count_unfilled > EMPTY_PACKET_TRESHOLD)
		{
			send_msg(&stam, sizeof(int), FRAME_ATTR);
		}
		else
			driver_queue.is_frame_requested = 0;

	}
	else if(info->attrs[INIT_ATTR])
	{
		printd("NETLINK -->  msg_cb::INIT_ATTR\n");
	}
	else
	{
		printd("NETLINK -->  msg_cb::NOT-SUPPORTED!\n");
		printd("Not supporting attribute.\n");
	}


	printd("---END NETLINK MSG CALLBACK---\n");
	return 0;
}

/*****************************************************************************/
/*************************** BASE NETLINK FUNCTIONS **************************/
/*****************************************************************************/

/* REGISTER NETLINK FAMILY *************************************************/
int register_nlfamily(void)
{
	int rc;
	printd("Registering the family.\n");
	rc = genl_register_family(&ex_genl_family);
	if (rc != 0)
	{
		printd("Failed to register family! Ret was %d.\n", rc);
		return -1;
	}

	printd("Family registered. Now registering ops for it.\n");

	rc = genl_register_ops(&ex_genl_family, &ex_gops);
	if (rc != 0)
	{
		printd("Failed to register ops! Ret was %d.\n", rc);
		return -1;
	}

	printd("Ops registered for the family successfully.\n");

	printd("Registering the standard multicast group.\n");
	rc = genl_register_mc_group(&ex_genl_family, &ex_mc_grp);
	if (rc != 0)
	{
		printd("Failed to register ops! Ret was %d.\n", rc);
		return -1;
	}
	printd("The standard multicast group was registered successfully.\n");

	return 0;
}

/* UNREGISTER NETLINK FAMILY *************************************************/
int unregister_nlfamily(void)
{
	int rc = 0;

	/* Unregister family */
	rc = genl_unregister_family(&ex_genl_family);
	if (rc != 0)
	{
		printd("Failed to unregister the family! Ret was %d.\n", rc);
		return -1;
	}

	return 0;
}

/*****************************************************************************/
/**************************** MODULE FUNCTIONS *******************************/
/*****************************************************************************/


/* OPEN DEVICE *************************************************************/
static int device_open(struct inode *inode, struct file *file)
{
	/* We don't want to talk to two processes at the same time */
	if (deviceOpen)
		return -EBUSY;

	/* Mark that device is opened. */
	deviceOpen++;
	printd("DEVICE OPEN --> no. of opened device %d", deviceOpen);
	/* Increase using processes counter. */
	try_module_get(THIS_MODULE);

	return 0;
}


/* RELEASE DEVICE **********************************************************/
static int device_release(struct inode *inode, struct file *file)
{
	/* Ready for our next caller. */
	deviceOpen--;
	printd("DEVICE RELEASE --> no. of opened device %d", deviceOpen);

	/* Decrease using processes counter. */
	module_put(THIS_MODULE);
	return 0;
}


/* READ DEVICE *************************************************************/
static ssize_t device_read(struct file *filp,	/* see include/linux/fs.h   */
			   char *buffer,	/* buffer to fill with data */
			   size_t length,	/* length of the buffer     */
			   loff_t * offset)
{
	printd("Sorry, this operation isn't supported.\n");
	return -EINVAL;
}


static ssize_t
device_write(struct file *filp, const char *buff, size_t len, loff_t * off)
{
	printd("Sorry, this operation isn't supported.\n");
	return -EINVAL;
}


/* IOCTL FUNCTION **********************************************************/
int device_ioctl(struct inode *inode, struct file *file,
		         unsigned int ioctl_num, unsigned long ioctl_param)
{
	/* Switch according to the ioctl called. */
	int stam;
	struct v4l2_format *s_fmt;
	struct v4l2_capability *cap;
	struct v4l2_requestbuffers *req;
	void *ret;
	printd("---START IOCTL REQUEST---\n");
	switch (ioctl_num)
	{
		case VIDIOC_QUERYCAP:
		{
			cap = (struct v4l2_capability *)ioctl_param;
			stam =  0 | V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
			copy_to_user(&cap->capabilities, &stam, sizeof(int));
			return 0;
		}
		case VIDIOC_G_FMT:
		{
			printd("device_ioctl::VIDIOC_G_FMT\n");

			fmt_ptr = (char *)ioctl_param;

			if(send_msg(&stam, sizeof(uint32_t), G_FMT_ATTR))
				return -1;

			wait_for_completion(&cond);

			//init_queue_buffers();

			copy_to_user(fmt_ptr,data.msg, data.length);

			printd("END device_ioctl::VIDIOC_G_FMT\n");

			return 0;
		}

		case VIDIOC_S_FMT:
		{
			s_fmt = (struct v4l2_format *)ioctl_param;
			stam = V4L2_PIX_FMT_YUV420;
			copy_to_user(&s_fmt->fmt.pix.pixelformat, &stam, sizeof(int));
			stam = 320;
			copy_to_user(&s_fmt->fmt.pix.width, &stam, sizeof(int));
			stam = 240;
			copy_to_user(&s_fmt->fmt.pix.height, &stam, sizeof(int));
			return 0;
		}

		case VIDIOC_STREAMON:
		{
			printd("device_ioctl::VIDIOC_STREAMON\n");
			if(send_msg(&stam, sizeof(uint32_t), STREAMON_ATTR))
				return -1;

			wait_for_completion(&cond);

			// request for first frame
			send_msg(&stam, sizeof(uint32_t), FRAME_ATTR);
			driver_queue.is_frame_requested = 1;
			printd("END device_ioctl::VIDIOC_STREAMON\n");
			return 0;
		}

		case VIDIOC_STREAMOFF:
		{
			printd("device_ioctl::VIDIOC_STREAMOFF\n");

			if(send_msg((void *)ioctl_param, sizeof(uint32_t), STREAMOFF_ATTR))
				return -1;

			printd("END device_ioctl::VIDIOC_STREAMOFF\n");
			return 0;
		}

		case VIDIOC_DQBUF:
		{
			printd("device_ioctl::VIDIOC_DQBUF\n");
			ret = dequeue_buffer();
			copy_to_user((void **)ioctl_param, &ret, sizeof(void *));
			printd("device_ioctl::queue empty count %d\n", driver_queue.count_unfilled);
			printd("device_ioctl::queue filled count %d\n", driver_queue.count);
			if(driver_queue.count_unfilled > EMPTY_PACKET_TRESHOLD && !driver_queue.is_frame_requested)
			{
				if(send_msg(&stam, sizeof(uint32_t), FRAME_ATTR))
					return -1;
				driver_queue.is_frame_requested = 1;
			}
			printd("END device_ioctl::VIDIOC_DQBUF\n");
			return (ret == NULL) ? -1 : 0;
		}

		case VIDIOC_QBUF:
		{
			printd("device_ioctl::VIDIOC_QBUF\n");

			stam = enqueue_buffer((void *) ioctl_param);
			printd("device_ioctl::pointer %x\n", (unsigned long)ioctl_param);
			printd("device_ioctl::queue empty count %d\n", driver_queue.count_unfilled);
			printd("device_ioctl::queue filled count %d\n", driver_queue.count);

			printd("END device_ioctl::VIDIOC_QBUF\n");
			return stam;
		}

		case VIDIOC_REQBUFS:
		{
			init_queue_buffers();
			printd("device_ioctl::VIDIOC_REQBUF\n");
			stam = DRIVER_QUEUE;
			req = (struct v4l2_requestbuffers *)ioctl_param;
			copy_to_user(&req->count, &stam, sizeof(int));
			printd("END device_ioctl::VIDIOC_REQBUF\n");
			return 0;
		}
	}
	printd("---END IOCTL REQUEST---\n");
	return 0;
}

/*****************************************************************************/
/************************** BASE MODULE FUNCTIONS ****************************/
/*****************************************************************************/

/* INITIALIZE MODULE *******************************************************/
static int __init mod_ex_init(void)
{
	int ret = 0;

	printd("---INIT MODULE---\n");

	// allocate all driver memory at once
	mem = kmalloc(sizeof(net_link_data) * DRIVER_QUEUE,GFP_KERNEL);
	if( mem == NULL)
		goto out_nothing;

	/* Register the character device. */
	ret = register_chrdev(MAJOR_NUM, DEVICE_NAME, &Fops);
	if (ret < 0)
	{
		printd("Can't register device\n");
		goto out_kfree;
	}

	// set data message to be null
	data.length = 1;
	data.msg[0] = 0;

	/* Initialize mutex for netlink communication */
	init_completion(&cond);

	net = NULL;

	/* Register netlink family */
	if(register_nlfamily() < 0)
	{
		printd("Unable to register family.\n");
		goto out_unalloc_region;
	}

	/* Finish */
	printd("Device registered\n");
	printd("---END INIT MODULE---\n\n");


	return ret;


	out_unalloc_region:
		unregister_chrdev(MAJOR_NUM, DEVICE_NAME);
	out_kfree:
		kfree(mem);
	out_nothing:
		return ret;
}


/* CLEANUP MODULE **********************************************************/
static void __exit mod_ex_exit(void)
{
	/* Unregister net link family */
	unregister_nlfamily();
	printd("Netlink family unregistered\n");

	/* Unregister char device */
	unregister_chrdev(MAJOR_NUM, DEVICE_NAME);

	printd("Device cleaned\n");

	return;
}

module_init(mod_ex_init);
module_exit(mod_ex_exit);

